#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#一条规划，回到middle位置再送食物，会掉电，所以延时后回到middle
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


def scale_trajectory_speed(traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

def move_to_multiple_targets():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 -14/-45/90/-45/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = 0.1038794732882471
    pose1.position.y = -0.055993214192435026
    pose1.position.z = 0.2876041751758597
    pose1.orientation.x = 7.895216365982998e-06
    pose1.orientation.y = 4.925607556231088e-05
    pose1.orientation.z = -0.12184804269604492
    pose1.orientation.w = 0.9925487655539387
    waypoints.append(pose1)

    # 第2个目标位姿 -28/-45/90/-45/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.08367758643029065
    pose2.position.y = -0.1086808498137246
    pose2.position.z = 0.2875887574546225
    pose2.orientation.x = 1.927121538709558e-05
    pose2.orientation.y = 6.971049619118313e-05
    pose2.orientation.z = -0.24189446329796155
    pose2.orientation.w = 0.97030256280959
    waypoints.append(pose2)

    # 第3个目标位姿 -28/-50/90/15/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = -0.03502840972419606
    pose3.position.y = -0.045589483814465884
    pose3.position.z = 0.10614045818885712
    pose3.orientation.x = 0.1116759438658971
    pose3.orientation.y = 0.44806053427619247
    pose3.orientation.z = -0.21459909236682168
    pose3.orientation.w = 0.8606494470695548
    waypoints.append(pose3)

    # 4 -28/-70/120/-12/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.005919275733848872
    pose4.position.y = -0.067335769649617
    pose4.position.z = 0.08514319292225526
    pose4.orientation.x = 0.07874286740695134
    pose4.orientation.y = 0.31590177237344613
    pose4.orientation.z = -0.22869782778735018
    pose4.orientation.w = 0.9174436956070898
    waypoints.append(pose4)

    # 5 -28/-65/121/-17/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.016198825353478746
    pose5.position.y = -0.07282416841740312
    pose5.position.z = 0.08179027140107833
    pose5.orientation.x = 0.08069348056375333
    pose5.orientation.y = 0.3238641972342192
    pose5.orientation.z = -0.22806551965504196
    pose5.orientation.w = 0.9146511152832089
    waypoints.append(pose5)

    # 第6个目标位姿 -28/-68/128/-27/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.032492179697122675
    pose6.position.y = -0.0814728216582047
    pose6.position.z = 0.08401846064911692
    pose6.orientation.x = 0.06864384957945104
    pose6.orientation.y = 0.27555892205004473
    pose6.orientation.z = -0.23195179933670168
    pose6.orientation.w = 0.9303513665158974
    waypoints.append(pose6)

    # 第7个目标位姿 -28/-70/135/-36/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.0446287643769399
    pose7.position.y = -0.08791412462470531
    pose7.position.z = 0.08061218194929645
    pose7.orientation.x = 0.06051325339911822
    pose7.orientation.y = 0.242909963020376
    pose7.orientation.z = -0.23418814928531106
    pose7.orientation.w = 0.9394087538248804
    waypoints.append(pose7)


    # 第8个目标位姿 -28/-70/141/-50/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.06423452902701184
    pose8.position.y = -0.09829584973459393
    pose8.position.z = 0.09410037803385392
    pose8.orientation.x = 0.04403927409919545
    pose8.orientation.y = 0.17677706686087613
    pose8.orientation.z = -0.23775784600096436
    pose8.orientation.w = 0.9540867977463388
    waypoints.append(pose8)

    # 第9个目标位姿 -28/-72/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.0872709979971224
    pose9.position.y = -0.11052866737323061
    pose9.position.z = 0.1530968348692439
    pose9.orientation.x = -2.2012373911066797e-05
    pose9.orientation.y = -9.595675516130216e-05
    pose9.orientation.z = -0.2417885076640756
    pose9.orientation.w = 0.9703289688911364
    waypoints.append(pose9)

    # # 第9.5个目标位姿 -28/-69/154/-90/0 举平
    # pose99 = geometry_msgs.msg.Pose()
    # pose99.position.x = 0.09283120272437292
    # pose99.position.y = -0.11345799827805057
    # pose99.position.z = 0.16258990409537252
    # pose99.orientation.x = -0.01053572946781694
    # pose99.orientation.y = -0.042308901440091425
    # pose99.orientation.z = -0.24151692254776375
    # pose99.orientation.w = 0.9694165933109311
    # waypoints.append(pose99)

    # 第10个目标位姿 -14/-72/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.1078137015424794
    pose10.position.y = -0.057002013797603834
    pose10.position.z = 0.15304097497404734
    pose10.orientation.x = 7.721304036228014e-06
    pose10.orientation.y = 4.7830876074295094e-05
    pose10.orientation.z = -0.12190999444880327
    pose10.orientation.w = 0.9925411582932376
    waypoints.append(pose10)

    # 第11个目标位姿 middle
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.11075168589927481
    pose11.position.y = -1.091277667118024e-05
    pose11.position.z = 0.2875988062936282
    pose11.orientation.x = 1.8382147442206865e-06
    pose11.orientation.y = 8.132110012412454e-05
    pose11.orientation.z = -2.4453694472133078e-05
    pose11.orientation.w = 0.9999999963927584
    waypoints.append(pose11)

    # 第12个目标位姿 0/40/0/-40/0
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.3035704582748756
    pose12.position.y = -4.6019088696090045e-05
    pose12.position.z = 0.30536264212765296
    pose12.orientation.x = 1.8345570569793093e-06
    pose12.orientation.y = -7.180863568924419e-05
    pose12.orientation.z = -6.589662785214818e-05
    pose12.orientation.w = 0.9999999952488945
    waypoints.append(pose12)

    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,  
        0.01,        
        False)       
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,1.2)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    # rospy.sleep(3)
    # arm_group.set_named_target("middle")
    # plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

if __name__ == '__main__':
    try:
        move_to_multiple_targets()
    except rospy.ROSInterruptException:
        pass
